/*
The surface-physics project: http://code.google.com/p/surface-physics/
Adds real world physics based on the Bullet physics engine, to bases in the Orbiter space
flight simulation software.

Written by Abhijit Nandy
License : Apache License 2.0 

Bump_3WheeledVehicle.cpp
Bump_3WheeledVehicle management implementation.


*/


/* Class declaration */
#include "Bump_3WheeledVehicle.h"



DerivedRegister<Bump_3WheeledVehicle> Bump_3WheeledVehicle::reg("Bump_3WheeledVehicle");



/**
 * Constructor for this derived class
 * Will be used if some attachment specific needs to be initialized comes up
 * Do NOT allocate memory here, let init() do that
 *
 * It is in fact currently used for a temporary vehicle simulation class which
 * should actually be derived from Bump_3WheeledVehicle.
 */
Bump_3WheeledVehicle::Bump_3WheeledVehicle()
{
	//Vehicle
	m_carChassis = NULL;
	m_vehicle = 0;
	m_wheelShape = 0;
	mass = 1500.f;



	//Engine parameters
	maxEngineForce = 3000.f;//this should be engine/velocity dependent
	maxBreakingForce = 500.f;
	gBreakingForce = 0.f;
	gVehicleSteering = 0.f;
	gEngineForce = 0.f;  //initial force

	//Transmission : simple gear model which varies engine force only
	max_gear = 6; //Reverse and neutral indicated by 'R' & 'N'
	min_gear = 1;
	m_gear = 1;

	//Steering parameters
	steeringIncrement = 0.04f;
	steeringClamp = 0.3f;

	/**
	 * Wheel parameters : Wheels continue to penetrate triangle mesh, not because of physics model,
	 * but because wheel mesh and physics wheel not aligned(physics wheel is more inwards,
	 * collision mesh wheel more outwards : there is actually no penetration)
	 */
	wheelRadius = 1.5f;
	wheelWidth = 1.182f;	 //actual width is 2.364(twice)
	wheelFriction = 1500;
	wheelFwdBackDist = 2.7f;
    wheelLeftRightDist = 1.3f;
	connectionHeight = 1.2f;
	rightIndex = 0;
	upIndex = 1;
	forwardIndex = 2;
	wheelDirectionCS0.setValue(0,-1,0);
	wheelAxleCS.setValue(-1,0,0);

	//Suspension parameters
	suspensionStiffness = 50.f;
	suspensionDamping = 20.0f;
	suspensionCompression = 2.4f;
	rollInfluence = 0.08f;//1.0f;
	suspensionRestLength = 0.9f;

	//Heights and initial location
	initialVehicleHeight = 2.0f; //Height from which vehicle is dropped on terrain at physics start
	chassisHeight = 1.2f; //Height of chassis above vehicle origin
	chassisHalfLength = 3.0f;
	chassisHalfWidth = 2.0f;
	chassisHalfHeight = 1.0f;

	//Raycast vehicle cant yet be deleted from physics world, though the VESSEL may be destroyed in
	//Orbiter, so deactivate it
	deleteIfInactive = false;



}


/**
 * Destructor, does nothing currently
 */
Bump_3WheeledVehicle::~Bump_3WheeledVehicle()
{}


/**
 * Initialize a base attachment.
 * Creates a new BulletBase Attachment and inserts it in the Orbiter world & Bullet World
 *
 * The search or create functionality provided here has to be part of this derived class as
 * for the other derived class, there is no search required(Meshes must be created always)
 *
 * A attachment needs to be added to the BulletBase. There can be at most MAX_OBJECTS number of attachments
 * The check for whether the number of attachments is < MAX_OBJECTS is already done at
 * Bump_BulletBase::readCreatePhysicsFileObject()
 *
 * Also a single attachment point needs to be created in the vessel that will be attached.
 *
 * Both attachment points may need to be deleted midway, which brings up the question of how to delete
 * an attachment point in the middle of atBase[]. When attachment points are compacted, then all
 * attached objects have to be updated as they store the position in atBase[] for their VESSELs.
 *
 *
 *
 * TODO: Currently only searches and attaches, no create.
 * TODO: Check if vessel attachment point already exists, also remove attachment point whenever detached
 *
 */
int
Bump_3WheeledVehicle::init(Bump_BulletBase *bb, char *strObjName,  OBJHANDLE hObj)
{
	int rv;
	
	bumpBase = bb;
	strcpy(strObjectName, strObjName);

	sprintf(boutput, "Bump_3WheeledVehicle::init: Initializing %s", strObjectName);
	oapiWriteLog(boutput);

	//Create Orbiter object
	rv = attachOrbiterVESSEL();
	if(rv){
		sprintf(boutput, "Bump_3WheeledVehicle::init: ERROR : Attaching Orbiter VESSEL failed %s", strObjectName);
		oapiWriteLog(boutput);
		return 1;
	}


	//Create terrain patch from initial equatorial position of vessel
	if(bumpBase->terrainGenerationEnabled){
		ground = new Bump_TerrainPatch(8, 8, 0.000002f, 0.000002f, 0.000003f, 0.000003f, 10.f);

		rv = ground->createPatch(bumpBase, lng, lat);
		if(rv){
			//detach VESSEL that would already have been attached at this point
			if(detachOrbiterVESSEL()){
				//OK, now something is *REALLY* wrong, just inform & return...
				sprintf(boutput, "Bump_3WheeledVehicle::init: ERROR : Detaching Orbiter VESSEL failed after failure to " \
						"create physics counterpart %s", strObjectName);
				oapiWriteLog(boutput);
			}

			sprintf(boutput, "Bump_3WheeledVehicle::init: ERROR : Could not add ground for %s", strObjectName);
			oapiWriteLog(boutput);
			return 2;
		}
	}


	//Create physics counterpart, if this fails, should detach Orbiter VESSEL to return it to initial state
	rv = createBulletRigidBody();
	if(rv){
		//detach VESSEL that would already have been attached at this point
		if(detachOrbiterVESSEL()){
			//OK, now something is *REALLY* wrong, just inform & return...
			sprintf(boutput, "Bump_3WheeledVehicle::init: ERROR : Detaching Orbiter VESSEL failed after failure to " \
					"create physics counterpart %s", strObjectName);
			oapiWriteLog(boutput);
		}

		sprintf(boutput, "Bump_3WheeledVehicle::init: ERROR : Creating rigid body failed %s", strObjectName);
		oapiWriteLog(boutput);
		return 3;
	}

	//Wheel rendering
	m_ShapeDrawer = bump.getShapeDrawer();
	bump.addExternalRenderObject(this);
	//bump.registerExternalRenderFunc(p);


	//get vessel structure and set controlled VESSEL's state to landed
	VESSELSTATUS2 vs;
	memset(&vs, 0, sizeof(VESSELSTATUS2));
	vs.version = 2;

	vAttached->GetStatusEx(&vs);
	vs.status = 1;
	vAttached->DefSetStateEx(&vs);



	//Only after Orbiter AND Bullet successful, set physics to true
	physicsInited = true;


	return 0;
}


/**
 *
 */
int
Bump_3WheeledVehicle::prestep(double SimT, double SimDT, double MJD)
{
	//m_carChassis->applyGravity();

	return 0;

}




/**
 * Updates a base attachment
 * Primarily move the vessel mesh into position according to latest transform by setting
 * attachment point parameter.
 * Needs to read and process any commands too from the containing base
 */
int
Bump_3WheeledVehicle::poststep(double SimT, double SimDT, double MJD)
{

	btRigidBody *rb;
	VECTOR3 global;
	btScalar m_temp[16];


	//sprintf(boutput, "Bump_3WheeledVehicle::poststep: The next sim step will advance the sim by %f secs", SimDT);
	//oapiWriteLog(boutput);

	if(physicsInited){



		/* Graphics and Orbiter updating stuff can be either in poststep(), then
		 * accounted for in Orbiter Sim in next sim step, or prestep(), in which case
		 * they are accounted for in the upcoming simstep. Physics world updates can
		 * also be pushed to poststep() or prestep().
		 *
		 * Need to decide.
		 *
		 * Challenging to push Bullet's updates into Orbiter at same sim step unless
		 * Bullet update finished before Orbiter which nullifes multi-threading advantages
		 * So Bullet updates can be pushed to Orbiter for consideration in next sim step.
		 *
		 * Then Bullet always behind Orbiter by the current Orbiter simtime, which maybe ok
		 *
		 * This is true even if physics stuff done in prestep(), as long as multi-threading
		 * is used, Bullet's results cant be pushed to Orbiter as long as both update themselves
		 * simultaneously.
		 *
		 * Sequential operation of Bullet could be made faster thru multithreaded support so
		 * Bullet has its changes ready before Orbiter steps, then render will show correctly.
		 *
		 */

		//Graphics stuff, should be in poststep()-------------------------------------

		//Main world transform of the vehicle
		rb = m_vehicle->getRigidBody();
		rb->getCenterOfMassTransform().getOpenGLMatrix(m);



		bumpBase->bb->SetAttachmentParams(atBase,
						 _V(m[12] ,
							m[13],
							m[14] ),
						 _V(m[4], m[5], m[6]),
						_V(m[8], m[9], m[10]));




		for (int i=0;i<m_vehicle->getNumWheels();i++){

			//synchronize the wheels with the (interpolated) chassis worldtransform
			m_vehicle->updateWheelTransform(i, true);

			m_vehicle->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(m_temp);
			bumpBase->bb->Local2Global( _V(m_temp[12],m_temp[13], m_temp[14]),	global);
			vAttached->Global2Local(global, bbv->local[i]);

			bbv->rot[i] = m_vehicle->getWheelInfo(i).m_rotation;

		}


		//Insert Engine info, move to vehicle class
		bbv->gear = m_gear;
		bbv->speed = m_vehicle->getCurrentSpeedKmHour();
		bbv->steering = m_vehicle->getSteeringValue(0);

		bbv->updateVessel = true;


		//Physics stuff, should be in prestep()---------------------------------

		processCommand();


		//Update vehicle
		int wheelIndex = 1;
		m_vehicle->applyEngineForce(gEngineForce,wheelIndex);
		m_vehicle->setBrake(gBreakingForce,wheelIndex);
		wheelIndex = 2;
		m_vehicle->applyEngineForce(gEngineForce,wheelIndex);
		m_vehicle->setBrake(gBreakingForce,wheelIndex);

		wheelIndex = 0;
		m_vehicle->setSteeringValue(gVehicleSteering,wheelIndex);
		//wheelIndex = 1;
		//m_vehicle->setSteeringValue(gVehicleSteering,wheelIndex);


		//Update terrain
		if(bumpBase->terrainGenerationEnabled){

			//oapiEquToGlobal(base->hObjRefBody, lng, lat, base->rad, &gposSurfPoint);

			//oapiLocalToEqu(bumpBase->hObjRefBody, _V(m[12], m[13], m[14]), &lng, &lat, &rad);

			bumpBase->bb->Local2Global(_V(m[12], m[13], m[14]), gposVessel);
			oapiGlobalToEqu(bumpBase->hObjRefBody, gposVessel, &lng, &lat, &rad);

			/*sprintf(boutput, "Bump_3WheeledVehicle::poststep: Converted (%f,%f,%f) to lolara(%f,%f,%f)",
					m[12], m[13], m[14], lng, lat, rad);
			oapiWriteLog(boutput);*/

			int t = ground->updatePatch(bumpBase, lng, lat, rad);
		}


	}//physics inited

	return 0;
}


/**
 *	Process keyboard input any other messages
 */
int
Bump_3WheeledVehicle::processCommand()
{
	if(bbv->validKeyInput){

		if(bbv->keyInput.down){

			int rv;

			//Command
			switch (bbv->keyInput.key)
			{
				case OAPI_KEY_UP:
					m_carChassis->setActivationState(DISABLE_DEACTIVATION);
					gBreakingForce = 0.f;
					switch(m_gear){
						case 'N': gEngineForce = 0;					break;
						case 'R': gEngineForce =-maxEngineForce/4;	break;
						default:  gEngineForce = maxEngineForce/(max_gear/m_gear);	break;
					}
					break;

				case OAPI_KEY_DOWN:
					gBreakingForce = maxBreakingForce;
					gEngineForce = 0.f;
					break;

				case OAPI_KEY_RIGHT:
					gVehicleSteering += steeringIncrement;
					if (	gVehicleSteering > steeringClamp)
							gVehicleSteering = steeringClamp;
					//bbv->keyInput.key = NO_KEY;				//A possible syncing and message passing mechanism
					break;

				case OAPI_KEY_LEFT:
					gVehicleSteering -= steeringIncrement;
					if (	gVehicleSteering < -steeringClamp)
							gVehicleSteering = -steeringClamp;
					//bbv->keyInput.key = NO_KEY;
					break;

				case OAPI_KEY_U:
					reset();
					bbv->keyInput.key = NO_KEY;
					break;

				case OAPI_KEY_Z:
					m_carChassis->setActivationState(DISABLE_DEACTIVATION);
					bbv->keyInput.key = NO_KEY;
					break;

				case OAPI_KEY_SPACE:
					gVehicleSteering = 0;
					break;

				case OAPI_KEY_W:
					if(m_gear == 'N' || m_gear == 'R')
						m_gear = 1;
					else
						m_gear = (m_gear+1) > max_gear ? m_gear : m_gear+1;
					bbv->keyInput.key = NO_KEY;
					break;

				case OAPI_KEY_S:
					if(m_gear == 'N' || m_gear == 'R')
						m_gear = 1;
					else
						m_gear = (m_gear-1) < min_gear ? m_gear : m_gear-1;
					bbv->keyInput.key = NO_KEY;
					break;

				case OAPI_KEY_A:
					m_gear = 'N';
					break;

				case OAPI_KEY_D:
					m_gear = 'R';
					break;

				case OAPI_KEY_G:
					//chks in control-map for vessel wrappers & removes, this causes del of attach pts in dtor of wrapper
					rv = bumpBase->dropBaseObject(bbv->vShuttle->GetHandle());
					if(rv){
						sprintf(boutput, "Bump_3WheeledVehicle::processCommand: ERROR : Failed to DROP %p", bbv->vShuttle->GetHandle());
						oapiWriteLog(boutput);
					}
					break;

				case OAPI_KEY_F10 :
					PhysicsEngine::focusBody = m_carChassis;
					break;

				case NO_KEY:
					gEngineForce = 0.f;
					gBreakingForce = 0.f;
					break;

				default:
					break;
			}
		}
		else{
			switch (bbv->keyInput.key)	//this key was released
			{
				case OAPI_KEY_W:			//gear changes
				case OAPI_KEY_S:
				case OAPI_KEY_A:
				case OAPI_KEY_D:
					gEngineForce = 0.f;
					gBreakingForce = 0.f;
					break;

				case OAPI_KEY_UP: 			//accelerator released
					gEngineForce = 0.f;
					break;

				case OAPI_KEY_DOWN:			//brake released
					gBreakingForce = 0.f;
					break;



				default:
					break;
			}


			bbv->keyInput.key = NO_KEY;
		}
	}

	bbv->validKeyInput = false;

	return 0;
}


/**
 * Deallocates any memory, Bump_3WheeledVehicle::exit() will be called
 * by the containing Bump_BulletBase.
 *
 * BUG: m_carChassis may not need to be deleted explicitly ?
 *
 * Delete attachment points for the BulletBase and Attached vessel
 */
int
Bump_3WheeledVehicle::exit()
{
	int rv;


	//Destroy physics counterpart
	rv = destroyBulletRigidBody();
	if(rv){
		sprintf(boutput, "Bump_3WheeledVehicle::exit: ERROR : Destroying rigid body failed for %s", strObjectName);
		oapiWriteLog(boutput);
	}


	//Remove Orbiter handles, detach vessel, update base
/*	rv = detachOrbiterVESSEL();
	if(rv){
		sprintf(boutput, "Bump_3WheeledVehicle::exit: ERROR : Detaching Orbiter VESSEL failed %s", strObjectName);
		oapiWriteLog(boutput);
	}*/

	//Base class members cleanup
	Bump_BaseObject::exit();

	return rv;

}


/**
 * Only resets the physics body. There is no need to re-attach
 *
 * Currently complete vehicle memory de-allocation is disabled due to m_actions[] in the
 * btDiscreteDynamicsWorld not being properly resized m_actions.size() fails in an assertion.
 *
 * This has to be reproduced in the Vehicle demo and asked in the Bullet forums.
 * Also test with re-spawn(reqd for Orulex) & deallocate memory options in Orbiter debugging options.
 *
 *
 * Meanwhile reset() causes the vehicle to turn back upright if toppled.
 *
 * TODO: Causes crash, fix destroyBulletRigidBody(),
 * Test in VehicleDemo
 */
int
Bump_3WheeledVehicle::reset()
{
	//Simply de-allocate & re-allocate the vehicle
	//destroyBulletRigidBody();


	//Remove Physics stuff (reverse order of creation)
	//int i;
	/*for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete collision shapes
	for (int j=0;j<m_collisionShapes.size();j++)
	{
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}

	m_collisionShapes.clear();*/


	/*
	bumpBase->m_dynamicsWorld->removeAction(m_vehicle);

	//The main rigid body : there may be a BUG here, this may have to be removed
	//from the dynamics world
	if (m_carChassis && m_carChassis->getMotionState()){
		delete m_carChassis->getMotionState();

		sprintf(boutput,"Bump_3WheeledVehicle::destroyBulletRigidBody: Motion state deleted for %s",
							strObjectName);
		oapiWriteLog(boutput);
	}


	bumpBase->m_dynamicsWorld->removeCollisionObject(m_carChassis);

	delete m_carChassis;

	//Vehicle specific objects
	delete m_vehicleRayCaster;
	delete m_vehicle;

	//These shapes were not added to m_collisionShapes[], so they have to be removed here
	delete chassisBoxShape;
	delete compoundChassisShape;
	delete m_wheelShape;

	createBulletRigidBody();
	*/

	btTransform chassisTrans;
	chassisTrans.setIdentity();

	rb = m_vehicle->getRigidBody();
	rb->getCenterOfMassTransform().getOpenGLMatrix(m);

	//Current position is new starting position, dont reset at sim start, starting position
	rposStartingVesselPos.x = m[12];
	rposStartingVesselPos.y = m[13];
	rposStartingVesselPos.z = m[14];


	//Pads are dug into the ground, will make vessel jump out if not 0ed
	if(rposStartingVesselPos.y < 0.f)
	   rposStartingVesselPos.y =chassisHeight;

	chassisTrans.setOrigin(btVector3((btScalar)rposStartingVesselPos.x,
									 (btScalar)rposStartingVesselPos.y,
									 (btScalar)rposStartingVesselPos.z));

	//Reset vehicle parameters,  do not set brake here.Causes wheel to lock up, let wheels rotate
	//till they touch ground again
	m_carChassis->setWorldTransform(chassisTrans);
	m_carChassis->setLinearVelocity(btVector3(0.f, 0.f, 0.f));
	m_carChassis->setAngularVelocity(btVector3(0.f, 0.f, 0.f));
	m_gear = 1;
	gVehicleSteering = 0.f;
	gEngineForce = 0.f;
	gBreakingForce = 0.f;





	return 0;
}


/**
 * Attach the VESSEL to the BulletBase in the Orbiter world
 *
 */
int
Bump_3WheeledVehicle::attachOrbiterVESSEL()
{
	unsigned int i;
	unsigned int numSimVessels = oapiGetVesselCount();
	OBJHANDLE hObj;
	char strVesselName[MAX_STRLEN], strAttID[8];
	int bbvCount = 0;
	VECTOR3 gposVesselPos;


	//Search the Orbiter sim for this vessel
	for (i = 0; i<numSimVessels; i++) {

		//Assuming hObj will be non-NULL
		hObj = oapiGetVesselByIndex(i);
		oapiGetObjectName(hObj, strVesselName, MAX_STRLEN);

		sprintf(boutput, "Bump_3WheeledVehicle::init: %3d. Found : %s", i+1, strVesselName);
		oapiWriteLog(boutput);

		//Does the vessel name match this object's name ?
		if(strcmp(strVesselName, strObjectName) == 0){

			//VESSEL not yet attached but soon will be, hopefully!
			vAttached = (VESSEL3*)oapiGetVesselInterface(hObj);

			//Store vessel's equ position for terrain patch creation
			vAttached->GetEquPos(lng, lat, rad);

			//Convert Vessel's global position into BulletBase relative position
			//Only the relative vessel position is actually stored, its not updated after this
			vAttached->GetGlobalPos(gposVesselPos);
			bumpBase->bb->Global2Local(gposVesselPos , rposStartingVesselPos);

		/*	rposStartingVesselPos.x = 5;
			rposStartingVesselPos.y = 0;
			rposStartingVesselPos.z = 0;*/


			//Create an attachment point at the base, sep parent/child lists exist, take next avail. pos.
			sprintf(strAttID, "BB%d", bumpBase->bb->AttachmentCount(FALSE));
			atBase = bumpBase->bb->CreateAttachment(false,
					rposStartingVesselPos,
					_V(0, -1, 0),
					_V(0, 0, 1),
					 strAttID);



			//Create an attachment point for the VESSEL	, -1 temp. for debug.
			atVessel = vAttached->CreateAttachment(true, _V(0, 0, 0),
															 _V(0, -1, 0),
															 _V(0, 0, 1),
															 "BBA_CHILD");

			//Attachment points are valid ?
			if(atBase && atVessel){

				//Try attaching, child hObj is first parameter
				if(bumpBase->bb->AttachChild(hObj, atBase, atVessel)){

					//Success!, attached
					sprintf(boutput, "Bump_3WheeledVehicle::init: Added %s to %s at attachment handle position %d",
										strObjectName, bumpBase->strBaseName, bumpBase->bb->AttachmentCount(FALSE));
					oapiWriteLog(boutput);

					bbv = (BulletBaseVehicle*)vAttached;

					//Return non-zero only if object was found and attached, anything else must return 1
					return 0;
				}
				else{ //Failure, could not attach
					sprintf(boutput,"Bump_3WheeledVehicle::init: ERROR : Attach failed: p:%d c:%d &p:%p, &c:%p",
							bumpBase->bb->AttachmentCount(FALSE),
							vAttached->AttachmentCount(TRUE),
							atBase,
							atVessel);
					oapiWriteLog(boutput);

					//Should NULL all imp ptrs, to prevent use
					physicsInited = false;
					bbv = NULL;
					vAttached = NULL;
					atBase = NULL;
					atVessel = NULL;
				}



			}
			else{
				//One of the attachment handles could not be created
				sprintf(boutput,"Bump_3WheeledVehicle::init: ERROR : Attachment handle creation failed: p:%d c:%d &p:%p, &c:%p",
											bumpBase->bb->AttachmentCount(FALSE),
											vAttached->AttachmentCount(TRUE),
											atBase,
											atVessel);
				oapiWriteLog(boutput);
			}

		}//end-if(strcmp...

	}//end-for...


	//If we reach here, the vessel was not found or there was an error during attachment, return error
	return 1;
}


/**
 * Detach the VESSEL from the BulletBase in the Orbiter world
 *
 */
int
Bump_3WheeledVehicle::detachOrbiterVESSEL()
{

	//Attachment points are valid ?
	if(atBase && atVessel){

		//Try detaching child
		if(bumpBase->bb->DetachChild(atBase)){
			//Success!, detached
			sprintf(boutput, "Bump_3WheeledVehicle::detachOrbiterVESSEL: Detached VESSEL %s from %s, "\
					"attachment count in base now %d",
					strObjectName, bumpBase->strBaseName, bumpBase->bb->AttachmentCount(FALSE));
			oapiWriteLog(boutput);

			//Delete the attachment handles
			if(!(bumpBase->bb->DelAttachment(atBase)) || !(vAttached->DelAttachment(atVessel))){
				sprintf(boutput,"Bump_3WheeledVehicle::init: ERROR : Deletion failed for 1 attachment point: p:%d c:%d &p:%p, &c:%p",
									bumpBase->bb->AttachmentCount(FALSE),
									vAttached->AttachmentCount(TRUE),
									atBase,
									atVessel);
				oapiWriteLog(boutput);
			}


			//These must be nulled after successful detach ONLY
			physicsInited = false;
			bbv = NULL;
			vAttached = NULL;
			atBase = NULL;
			atVessel = NULL;


		}
		else{ //Failure, could not detach
			sprintf(boutput,"Bump_3WheeledVehicle::init: ERROR : Detach failed: p:%d c:%d &p:%p, &c:%p",
					bumpBase->bb->AttachmentCount(FALSE),
					vAttached->AttachmentCount(TRUE),
					atBase,
					atVessel);
			oapiWriteLog(boutput);

			return 1;
		}

		oapiWriteLog(boutput);

	}
	else{
		//One of the attachment handles is not valid, cannot detach
		sprintf(boutput,"Bump_3WheeledVehicle::init: ERROR : Detachment failed due to invalid attachment handles: p:%d c:%d &p:%p, &c:%p",
									bumpBase->bb->AttachmentCount(FALSE),
									vAttached->AttachmentCount(TRUE),
									atBase,
									atVessel);
		return 1;
	}

	return 0;
}
/**
 * Create the physics counterpart in the Bullet world
 *
 */
int
Bump_3WheeledVehicle::createBulletRigidBody()
{



	btTransform chassisTrans, localTrans;
	bool isFrontWheel;


	//The compound shape for the chassis
	compoundChassisShape = new btCompoundShape();
	//bumpBase->m_collisionShapes->push_back(compoundChassisShape);

	//The chassis shape to be added to the compound shape
	chassisBoxShape = new btBoxShape(btVector3(chassisHalfWidth,
											chassisHalfHeight,
											chassisHalfLength));
	//bumpBase->m_collisionShapes->push_back(chassisBoxShape);

	//localTrans for the chassis shape
	//It effectively shifts the center of mass with respect to the chassis
	localTrans.setIdentity();
	localTrans.setOrigin(btVector3(0, chassisHeight,0));

	//Add chassis shape to compound shape
	compoundChassisShape->addChildShape(localTrans, chassisBoxShape);

	//Position the chassis inside the vehicle
	chassisTrans.setIdentity();

	//Pads are dug into the ground, will make vessel jump out if not 0ed
	if(rposStartingVesselPos.y < 0.f)
		rposStartingVesselPos.y =chassisHeight + 10.0;
	chassisTrans.setOrigin(btVector3((btScalar)rposStartingVesselPos.x,
									 (btScalar)rposStartingVesselPos.y,
									 (btScalar)rposStartingVesselPos.z));
	m_carChassis = bumpBase->localCreateRigidBody(mass, chassisTrans, compoundChassisShape);

	//Now the wheels, first the cylinder shape for the wheels
	m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth, wheelRadius, wheelRadius));

	//Create vehicle
	m_vehicleRayCaster = new btDefaultVehicleRaycaster(bumpBase->m_dynamicsWorld);
	m_vehicle = new btRaycastVehicle(m_tuning,m_carChassis,m_vehicleRayCaster);

	//Never deactivate the vehicle
	m_carChassis->setActivationState(DISABLE_DEACTIVATION);

	bumpBase->m_dynamicsWorld->addVehicle(m_vehicle);

	//choose coordinate system
	m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);

	//forward left wheel
	isFrontWheel=true;
	btVector3 connectionPointCS0(0.f, connectionHeight, wheelFwdBackDist);
	m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);

	//forward right wheel
	//connectionPointCS0 = btVector3(-wheelLeftRightDist, connectionHeight, wheelFwdBackDist);
	//m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);

	isFrontWheel = false;
	//backward left wheel
	connectionPointCS0 = btVector3(wheelLeftRightDist, connectionHeight, -wheelFwdBackDist);
	m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);

	//backward right wheel
	connectionPointCS0 = btVector3(-wheelLeftRightDist, connectionHeight, -wheelFwdBackDist);
	m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);

	//Set wheel properties for all 4 wheels
	for (int i=0;i<m_vehicle->getNumWheels();i++){
		btWheelInfo& wheel = m_vehicle->getWheelInfo(i);
		wheel.m_suspensionStiffness = suspensionStiffness;
		wheel.m_wheelsDampingRelaxation = suspensionDamping;
		wheel.m_wheelsDampingCompression = suspensionCompression;
		wheel.m_frictionSlip = wheelFriction;
		wheel.m_rollInfluence = rollInfluence;
	}

	m_carChassis->setGravity(btVector3((btScalar)bumpBase->G.x,
									   (btScalar)bumpBase->G.y,
									   (btScalar)bumpBase->G.z));

	return 0;
}


/*
 * Wheels rendering for the vehicle
 */
int
Bump_3WheeledVehicle::renderme(void)
{
	int i;
	btVector3 wheelColor(1,0,0);
	btVector3	worldBoundsMin,worldBoundsMax;
	bumpBase->m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);



	for (i=0;i<m_vehicle->getNumWheels();i++)
	{
		//synchronize the wheels with the (interpolated) chassis worldtransform
		m_vehicle->updateWheelTransform(i,true);
		//draw wheels (cylinders)
		m_vehicle->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(m);
		m_ShapeDrawer->drawOpenGL(m, m_wheelShape,wheelColor, bump.getDebugMode(),worldBoundsMin,worldBoundsMax);
	}

	return 0;
}

/**
 * De-allocates memory being consumed by the raycast vehicle so it can be
 * created again by createBulletRigidBody()
 *
 * This is a very important function. If memory is not freed up, Orbiter sim will not
 * exit properly and the Launchpad will not appear leading to user frustration!
 *
 * Here we de-allocate only the sub class specific stuff which have not been freed by
 * bump.clientResetScene(), which is called before Bump_BulletBase::exit();
 *
 * Thus rigid bodies(derived from CollisionObjects), motion states are already removed
 * by the time we come here, DO NOT remove them again !
 *
 * The stuff which should be removed are any sub class specific classes used to model
 * this object like m_vehicleRayCaster & m_vehicle. Also any collision shapes not pushed
 * into m_collisionShapes[] during init(), needs to be freed here, such as the wheelShape
 */
int
Bump_3WheeledVehicle::destroyBulletRigidBody()
{
	physicsInited = false;

	//Remove Physics stuff (reverse order of creation),
	//
/*	bumpBase->m_dynamicsWorld->removeAction(m_vehicle);

	//The main rigid body : there may be a BUG here, this may have to be removed
	//from the dynamics world
	if (m_carChassis && m_carChassis->getMotionState()){
		delete m_carChassis->getMotionState();

		sprintf(boutput,"Bump_3WheeledVehicle::destroyBulletRigidBody: Motion state deleted for %s",
							strObjectName);
		oapiWriteLog(boutput);
	}

	//m_carChassis->getColli
	//m_dynamicsWorld->removeCollisionObject(
	bumpBase->m_dynamicsWorld->removeCollisionObject(m_carChassis);

	delete m_carChassis;
*/
	//Vehicle specific objects
	delete m_vehicleRayCaster;
	delete m_vehicle;

	//These shapes were not added to m_collisionShapes[], so they have to be removed here
	//They were not added so that they could be specifically delete during reset()
	delete chassisBoxShape;
	delete compoundChassisShape;
	delete m_wheelShape;

	return 0;
}


/**
 * Transform debugging function, can rip through the dynamics world and get transforms in the worst case
 * Used for investigating issues arising from incorrect extraction of transforms from the Bullet structs
 */
void
Bump_3WheeledVehicle::getTransforms(void)
{

	const int numObjects = bumpBase->m_dynamicsWorld->getNumCollisionObjects();

	//Go through all collision objects
	for(int i=0;i<numObjects;i++){
		//Common properties
		btCollisionObject*	colObj = bumpBase->m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody*		body   = btRigidBody::upcast(colObj);
		const btCollisionShape* shape = colObj->getCollisionShape();

		if(body&&body->getMotionState()){
			btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();

			if(shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE){			//chassis
				myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m);

				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
				btTransform childTrans = compoundShape->getChildTransform(0);
				//childTrans.getOpenGLMatrix(pkt.childMat);
				shape = compoundShape->getChildShape(0);
				if(shape->getShapeType() == BOX_SHAPE_PROXYTYPE){
					const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
					//pkt.halfExtent = boxShape->getHalfExtentsWithoutMargin(); //with and without margin makes a difference in size

				}
			}
			else if(shape->getShapeType() == BOX_SHAPE_PROXYTYPE){		//collision box
				//const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
				//myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(pkt.m_box);
				//pkt.box_halfExtent = boxShape->getHalfExtentsWithoutMargin();
			}

		}
		else{
			if(shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE){			//chassis
				colObj->getWorldTransform().getOpenGLMatrix(m);

				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
				btTransform childTrans = compoundShape->getChildTransform(0);
				//childTrans.getOpenGLMatrix(pkt.childMat);
				shape = compoundShape->getChildShape(0);
				if(shape->getShapeType() == BOX_SHAPE_PROXYTYPE){
					const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
					//pkt.halfExtent = boxShape->getHalfExtentsWithMargin();

				}
			}
			else if(shape->getShapeType() == BOX_SHAPE_PROXYTYPE){		//collision box
				//const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
				//colObj->getWorldTransform().getOpenGLMatrix(pkt.m_box);
				//pkt.box_halfExtent = boxShape->getHalfExtentsWithMargin();
			}
		}
	}
}



